/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.SmartDashboard;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.Timer;

/**
 *
 * @author Administrator
 */
public class Arm {

    private CANJaguar m_shoulder;
    private CANJaguar m_elbow;
    private DriverStationEnhancedIO m_enhancedIO;
    private int m_shoulderWaldoChannel;
    private int m_elbowWaldoChannel;
    private double m_shoulderValue = 0;
    private double m_elbowValue = 0;
    private double m_waldoShoulderValue = 0;
    private double m_waldoElbowValue = 0;
    private double m_shoulderRequested = 0;
    private double m_elbowRequested = 0;
    // This is correct. The down value is greater than the up value on the Waldo.
    private static final double kDownShoulderVoltage = 2.4;
    private static final double kUpShoulderVoltage = 1.3;
    private static final double kRetractedElbowVoltage = 2.8;
    private static final double kExtendedElbowVoltage = 0.9;
    public static final double kDownShoulderPosition = 0.132;
    public static final double kUpShoulderPosition = 0.599;
    public static final double kRetractedElbowPosition = 0.531; // Changed 8/19/2011
    public static final double kExtendedElbowPosition = 0.987; // Changed 8/19/2011
    public static final double kElbowMinToBeCollecting = .4;
    public static final double kShoulderMinWhenElbowInCollecting = 0.151;
    private DriverStation m_ds = DriverStation.getInstance();

    public Arm(int shoulderCANDevice, int elbowCANDevice, int shoulderWaldoChannel, int elbowWaldoChannel) {
        try {
            m_shoulder = new CANJaguar(shoulderCANDevice, CANJaguar.ControlMode.kPosition);
            m_elbow = new CANJaguar(elbowCANDevice, CANJaguar.ControlMode.kPosition);
            init();

        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println("CAN Arm Init Error!");
        }
        m_enhancedIO = DriverStation.getInstance().getEnhancedIO();
        m_shoulderWaldoChannel = shoulderWaldoChannel;
        m_elbowWaldoChannel = elbowWaldoChannel;
    }

    public void init() {
        try {
            m_shoulder.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
            m_shoulder.setPID(1250, 0, 110);
            m_shoulder.configPotentiometerTurns(1);
            m_shoulder.configSoftPositionLimits(kUpShoulderPosition, kDownShoulderPosition);
            m_shoulder.enableControl();
            m_elbow.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
            m_elbow.setPID(700, 0, 50);
            m_elbow.configPotentiometerTurns(1);
            m_elbow.configSoftPositionLimits(kExtendedElbowPosition, kRetractedElbowPosition);
            m_elbow.enableControl();
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println("CAN Arm Init Error!");
        }
    }

    public void getValues() {
        try {
            m_shoulderValue = m_enhancedIO.getAnalogIn(m_shoulderWaldoChannel);
            m_waldoShoulderValue = m_shoulderValue;
            m_shoulderValue -= kDownShoulderVoltage;
            m_shoulderValue /= (kUpShoulderVoltage - kDownShoulderVoltage);
            if (m_shoulderValue < 0) {
                m_shoulderValue = 0;
            }
            if (m_shoulderValue > 1) {
                m_shoulderValue = 1;
            }

            m_elbowValue = m_enhancedIO.getAnalogIn(m_elbowWaldoChannel);
            m_waldoElbowValue = m_elbowValue;
            m_elbowValue -= kRetractedElbowVoltage;
            m_elbowValue /= (kExtendedElbowVoltage - kRetractedElbowVoltage);
            if (m_elbowValue < 0) {
                m_elbowValue = 0;
            }
            if (m_elbowValue > 1) {
                m_elbowValue = 1;
            }
        } catch (DriverStationEnhancedIO.EnhancedIOException e) {
            DriverStationPrinter.getInstance().println("Waldo Pos Read Error!");
        }
    }

    public void update() {
        getValues();
        double dMotorValueShoulder = m_shoulderValue;
        dMotorValueShoulder *= (kUpShoulderPosition - kDownShoulderPosition);
        dMotorValueShoulder += kDownShoulderPosition;
        m_shoulderRequested = dMotorValueShoulder;

        double dMotorValueElbow = m_elbowValue;
        dMotorValueElbow *= (kExtendedElbowPosition - kRetractedElbowPosition);
        dMotorValueElbow += kRetractedElbowPosition;
        m_elbowRequested = dMotorValueElbow;

        // Make sure that when the elbow is extended to collect that the shoulder stays
        // up enough to prevent the forarm from hitting the front of the robot.
        if ((dMotorValueElbow > kElbowMinToBeCollecting) && (dMotorValueShoulder < kShoulderMinWhenElbowInCollecting)) {
            dMotorValueShoulder = kShoulderMinWhenElbowInCollecting;
        }

        try {
            m_shoulder.setX(dMotorValueShoulder);
            Timer.delay(0.050);
            m_elbow.setX(dMotorValueElbow);
            Timer.delay(0.050);
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println("CAN Arm Pos Error!");
        }
    }

    // Must call getValues or update first on the next two methods.
    public double getSholderValue() {
        return m_shoulderValue;
    }

    public double getElbowValue() {
        return m_elbowValue;
    }

    /*
     *
     * @param shoulderPosition The position value for the shoulder in motor counts.
     * @param elbowPosition The position value for the elbow in motor counts.
     */
    public void setAutonomousArmPositions(double shoulderPosition, double elbowPosition) {
        if (m_ds.isAutonomous()) {
            // Make sure the shoulder position is in range.
            if (shoulderPosition > kUpShoulderPosition) {
                shoulderPosition = kUpShoulderPosition;
            } else if (shoulderPosition < kDownShoulderPosition) {
                shoulderPosition = kDownShoulderPosition;
            }
            // Make sure the elbow position is in range.
            if (elbowPosition > kExtendedElbowPosition) {
                elbowPosition = kExtendedElbowPosition;
            } else if (elbowPosition < kRetractedElbowPosition) {
                elbowPosition = kRetractedElbowPosition;
            }

            // Set the positions.
            try {
                m_shoulder.setX(shoulderPosition);
                Timer.delay(.050);
                m_elbow.setX(elbowPosition);
                Timer.delay(.050);
            } catch (CANTimeoutException e) {
                DriverStationPrinter.getInstance().println("CAN Auto Arm Pos Error!");
            }
        }
    }

    /**
     * Updates the information on the SmartDashboard.
     */
    public void updateSmartDashboard() {
        try {
            double shoulderPos = m_shoulder.getPosition();
            Timer.delay(0.050);
            double elbowPos = m_elbow.getPosition();
            Timer.delay(0.050);
            SmartDashboard.log(shoulderPos, "Shoulder Position");
            SmartDashboard.log(elbowPos, "Elbow Position");
            SmartDashboard.log(m_waldoShoulderValue, "Waldo Shoulder");
            SmartDashboard.log(m_waldoElbowValue, "Waldo Elbow");
            SmartDashboard.log(m_shoulderRequested, "Shoulder Requested");
            SmartDashboard.log(m_elbowRequested, "Elbow Requested");
            String shoulderFaults = "Not Checked";
            String elbowFaults = "Not Checked";

            if (Math.abs(shoulderPos) < (Math.min(kDownShoulderPosition, kUpShoulderPosition) / 2)) {
                shoulderFaults = "";
                short faults = m_shoulder.getFaults();
                if ((faults & CANJaguar.Faults.kBusVoltageFault.value) > 0) {
                    shoulderFaults += "Bus ";
                }
                if ((faults & CANJaguar.Faults.kCurrentFault.value) > 0) {
                    shoulderFaults += "Curr ";
                }
                if ((faults & CANJaguar.Faults.kGateDriverFault.value) > 0) {
                    shoulderFaults += "Gate ";
                }
                if ((faults & CANJaguar.Faults.kTemperatureFault.value) > 0) {
                    shoulderFaults += "Temp";
                }
                if (shoulderFaults.equals("")) {
                    shoulderFaults = "None";
                } else if (shoulderFaults.charAt(shoulderFaults.length() - 1) == ' ') {
                    shoulderFaults = shoulderFaults.substring(0, shoulderFaults.length() - 1);
                }
                Timer.delay(0.050);
            }

            if (Math.abs(elbowPos) < (Math.min(kRetractedElbowPosition, kExtendedElbowPosition) / 2)) {
                elbowFaults = "";
                short faults = m_elbow.getFaults();
                if ((faults & CANJaguar.Faults.kBusVoltageFault.value) > 0) {
                    elbowFaults += "Bus ";
                }
                if ((faults & CANJaguar.Faults.kCurrentFault.value) > 0) {
                    elbowFaults += "Curr ";
                }
                if ((faults & CANJaguar.Faults.kGateDriverFault.value) > 0) {
                    elbowFaults += "Gate ";
                }
                if ((faults & CANJaguar.Faults.kTemperatureFault.value) > 0) {
                    elbowFaults += "Temp";
                }
                if (elbowFaults.equals("")) {
                    elbowFaults = "None";
                } else if (elbowFaults.charAt(elbowFaults.length() - 1) == ' ') {
                    elbowFaults = elbowFaults.substring(0, shoulderFaults.length() - 1);
                }
                Timer.delay(0.050);
            }

            SmartDashboard.log(shoulderFaults, "Shoulder Faults");
            SmartDashboard.log(elbowFaults, "Elbow Faults");
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println("CAN Get Arm Pos Error!");
        }
    }
}

